﻿//
// ScorBotScript.cs
//
// Authors:
//    Claus Jørgensen <10229@iha.dk>
//    Lars Rønde Hansen <09548@iha.dk>
//
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;
using System.Threading;
using System.Diagnostics.Contracts;
using System.Threading.Tasks;

using Antlr.Runtime.Tree;
using Antlr.Runtime;

namespace RSD.Scripting
{
    using Robot;
    using Data;
    using External;

    /// <summary>
    /// Scripting engine to be used with <see cref="IRobotController"/>.
    /// </summary>
    public class ScorBotScript : IScorBotScript
    {
        #region Members

        /// <summary>
        /// Token => Action expression mapping.
        /// </summary>
        private readonly Dictionary<string, Action<CommonTree>> expressions;

        /// <summary>
        /// Robot controller.
        /// </summary>
        private readonly IRobotController robotController;

        /// <summary>
        /// Scale.
        /// </summary>
        private readonly IScale scale;

        /// <summary>
        /// Position repository.
        /// </summary>
        private readonly IPositionRepository positionRepository;

        /// <summary>
        /// ItemClassification repository.
        /// </summary>
        private readonly IItemClassificationRepository itemClassificationRepository;

        /// <summary>
        /// MeasuredItemRepository repository.
        /// </summary>
        private readonly IMeasuredItemRepository measuredItemRepository;

        /// <summary>
        /// Current item being measured.
        /// </summary>
        private MeasuredItem measuredItem;

        /// <summary>
        /// Event for every time a action is executed.
        /// </summary>
        public event EventHandler<ActionExecutedEventArgs> ActionExecuted;

        /// <summary>
        /// Occurs when a item is added to the inventory.
        /// </summary>
        public event EventHandler<ItemAddedEventArgs> ItemAdded;

        #endregion

        #region Constructor

        /// <summary>
        /// Initializes a new instance of the <see cref="ScorBotScript"/> class.
        /// </summary>
        /// <param name="robotController">The robot controller.</param>
        public ScorBotScript(IRobotController robotController, IScale scale,
            IPositionRepository positionRepository,
            IItemClassificationRepository itemClassificationRepository,
            IMeasuredItemRepository measuredItemRepository)
        {
            Contract.Requires(robotController != null);
            Contract.Requires(scale != null);
            Contract.Requires(positionRepository != null);
            Contract.Requires(itemClassificationRepository != null);
            Contract.Requires(measuredItemRepository != null);

            this.scale = scale;
            this.robotController = robotController;
            this.itemClassificationRepository = itemClassificationRepository;
            this.positionRepository = positionRepository;
            this.measuredItemRepository = measuredItemRepository;

            expressions = new Dictionary<string, Action<CommonTree>>();
            expressions.Add("INIT", Initialization);
            expressions.Add("HOME", Home);
            expressions.Add("REPEAT_START", Repeat);
            expressions.Add("MOVE", MoveByVector);
            expressions.Add("MOVE_C", MoveByCoordinate);
            expressions.Add("MOVE_SORT", MoveToSortingLocation);
            expressions.Add("ROLL", RollConveyorBelt);
            expressions.Add("OPEN_GRIPPER", OpenGripper);
            expressions.Add("CLOSE_GRIPPER", CloseGripper);
            expressions.Add("MEASURE_JAW", MeasureJaw);
            expressions.Add("MEASURE_WEIGHT", MeasureWeight);
            expressions.Add("STOP", StopMovement);
        }

        #endregion

        /// <summary>
        /// Invariants this instance.
        /// </summary>
        [ContractInvariantMethod]
        private void Invariant()
        {
            Contract.Invariant(robotController != null);
            Contract.Invariant(scale != null);
            Contract.Invariant(positionRepository != null);
            Contract.Invariant(itemClassificationRepository != null);
            Contract.Invariant(measuredItemRepository != null);
        }

        /// <summary>
        /// Walks the specified tree.
        /// </summary>
        /// <param name="tree">The tree.</param>
        private void Walk(CommonTree tree)
        {
            Contract.Requires(tree != null);

            if (tree.Token != null) // Root Node
            {
                if (expressions.ContainsKey(tree.Token.Text))
                {
                    expressions[tree.Token.Text](tree);
                }
                else
                {
                    throw new ScorBotScriptException(string.Format(
                        "Unknown Token '{0}'", tree.Token.Text));
                }
            }
        }

        #region Utility

        /// <summary>
        /// Converts a database position to a robot position.
        /// </summary>
        /// <param name="position">The database position.</param>
        /// <returns>The corrosponding robot position.</returns>
        private static Robot.Position ConvertToRobotPosition(Data.Position position)
        {
            if (position == null)
            {
                return new Robot.Position();
            }

            return new Robot.Position()
            {
                Joint = new int[]
                    {
                        position.X,
                        position.Y,
                        position.Z,
                        position.Pitch,
                        position.Roll
                    }
            };
        }

        /// <summary>
        /// Calculates the density.
        /// </summary>
        /// <param name="measuredItem">The measured item.</param>
        /// <returns>The calculated density.</returns>
        private static double CalculateDensity(MeasuredItem measuredItem)
        {
            int width = (int)Math.Round((measuredItem.ActualWidth / 10)) * 10;
            int height = (int)Math.Round((measuredItem.ActualHeight / 10)) * 10;
            int depth = (int)Math.Round((measuredItem.ActualDepth / 10)) * 10;

            double volume = width * height * depth;
            double mass = measuredItem.ActualWeight;

            return (mass / 1000) / (volume / 1000000);
        }

        /// <summary>
        /// Called when a action is executed.
        /// </summary>
        /// <param name="format">The format.</param>
        /// <param name="args">The args.</param>
        protected void OnActionExecuted(string format, params object[] args)
        {
            Contract.Requires(args != null);
            Contract.Requires(!string.IsNullOrEmpty(format));

            if (!string.IsNullOrEmpty(format) && (args != null))
            {
                string message = string.Format(format, args);

                if (!string.IsNullOrEmpty(message))
                {
                    OnActionExecuted(message);
                }
            }
        }

        /// <summary>
        /// Called when a action is executed.
        /// </summary>
        /// <param name="message">The message.</param>
        protected void OnActionExecuted(string message)
        {
            Contract.Requires(!string.IsNullOrEmpty(message));

            if (ActionExecuted != null && !string.IsNullOrEmpty(message))
            {
                ActionExecuted(this, new ActionExecutedEventArgs(message));
            }
        }

        /// <summary>
        /// Called when a item is added to the database.
        /// </summary>
        /// <param name="item">The item.</param>
        protected void OnItemAdded(MeasuredItem item)
        {
            Contract.Requires(item != null);

            if (ItemAdded != null && item != null)
            {
                ItemAdded(this, new ItemAddedEventArgs(item));
            }
        }

        #endregion

        #region IScorBotScript Members

        /// <summary>
        /// Gets a value indicating whether the scripting-engine is executing.
        /// </summary>
        /// <value>
        /// 	<c>true</c> if the scripting-engine is executing; otherwise, <c>false</c>.
        /// </value>
        public bool IsExecuting
        {
            get;
            set;
        }

        /// <summary>
        /// Executes the script.
        /// </summary>
        /// <param name="script">The script in plaintext.</param>
        public void ExecuteScript(string script)
        {
            ExecuteScript(new StringReader(script));
        }

        /// <summary>
        /// Executes the script.
        /// </summary>
        /// <param name="reader">TextReader for loading entire files or streams.</param>
        public void ExecuteScript(TextReader reader)
        {
            var input = new ANTLRReaderStream(reader);
            var lexer = new ScorBotScriptLexer(input);
            var tokens = new CommonTokenStream(lexer);
            var parser = new ScorBotScriptParser(tokens);

            try
            {
                var result = parser.program();
                var rootNode = result.Tree as CommonTree;

                if (rootNode.Children != null)
                {
                    foreach (var child in rootNode.Children.Cast<CommonTree>())
                    {
                        if (child != null)
                        {
                            Walk(child);
                        }
                    }
                }

                robotController.RunActionQueue();
            }
            catch (RecognitionException e)
            {
                throw new ScorBotScriptException(string.Format("{0}: {1}",
                    parser.GetErrorHeader(e), parser.GetErrorMessage(e, parser.TokenNames)));
            }
        }

        /// <summary>
        /// Checks syntax in script.
        /// </summary>
        /// <param name="reader">The script in plaintext.</param>
        public void CheckScript(string script)
        {
            CheckScript(new StringReader(script));
        }

        /// <summary>
        /// Checks syntax in script.
        /// </summary>
        /// <param name="reader">TextReader for loading entire files or streams.</param>
        public void CheckScript(TextReader reader)
        {
            var input = new ANTLRReaderStream(reader);
            var lexer = new ScorBotScriptLexer(input);
            var tokens = new CommonTokenStream(lexer);
            var parser = new ScorBotScriptParser(tokens);

            try
            {
                var tree = parser.program();
            }
            catch (RecognitionException e)
            {
                throw new ScorBotScriptException(string.Format("{0}: {1}",
                    parser.GetErrorHeader(e), parser.GetErrorMessage(e, parser.TokenNames)));
            }
        }

        #endregion

        #region ScorBotScript Expression Methods

        /// <summary>
        /// Action for the INIT token.
        /// </summary>
        /// <param name="tree">Tree containing parameters or sub-expression.</param>
        private void Initialization(CommonTree tree)
        {
            Contract.Requires(tree != null);

            robotController.EnqueueAction(r =>
            {
                OnActionExecuted("Initializing...");
                r.Initialization(InitializationMode.Online);
                OnActionExecuted("Initialized.");
            });
        }

        /// <summary>
        /// Action for the HOME token.
        /// </summary>
        /// <param name="tree">Tree containing parameters or sub-expression.</param>
        private void Home(CommonTree tree)
        {
            Contract.Requires(tree != null);

            char robotAxis;
            if (char.TryParse(tree.GetChild(0).Text, out robotAxis))
            {
                robotController.EnqueueAction(r =>
                {
                    OnActionExecuted("Homing axis '{0}'...",
                        Enum.GetName(typeof(Axis), (Axis)robotAxis));

                    r.Home((Axis)robotAxis);

                    OnActionExecuted("Finished homing axis '{0}'.",
                        Enum.GetName(typeof(Axis), (Axis)robotAxis));
                });
            }
        }

        /// <summary>
        /// Action for the REPEAT_START token.
        /// </summary>
        /// <param name="tree">Tree containing parameters or sub-expression.</param>
        private void Repeat(CommonTree tree)
        {
            Contract.Requires(tree != null);

            IsExecuting = true;

            if (tree.Children != null)
            {
                var children = tree.Children.Cast<CommonTree>();

                int count = 0;
                if (int.TryParse(tree.GetChild(0).Text, out count))
                {
                    for (int index = 0; index < count; index++)
                    {
                        measuredItem = new MeasuredItem();

                        foreach (var child in children.Skip(1))
                        {
                            if (child != null)
                            {
                                Walk(child);
                            }
                        }
                    }
                }
            }

            robotController.RunActionQueue();

            IsExecuting = false;
        }

        /// <summary>
        /// Action for the MOVE token.
        /// </summary>
        /// <param name="tree">Tree containing parameters or sub-expression.</param>
        private void MoveByVector(CommonTree tree)
        {
            Contract.Requires(tree != null);

            string vectorName = tree.GetChild(0).Text;

            if (vectorName.Length >= 2)
            {
                vectorName = vectorName.Substring(1, vectorName.Length - 2);
            }

            var position = positionRepository.GetPositionByName(vectorName);
            if (position != null)
            {
                robotController.EnqueueAction(r => MoveByVectorAction(vectorName, position, r));
            }
        }

        /// <summary>
        /// Action for the MOVE_C token.
        /// </summary>
        /// <param name="tree">Tree containing parameters or sub-expression.</param>
        private void MoveByCoordinate(CommonTree tree)
        {
            Contract.Requires(tree != null);

            bool success = false;
            var position = new Robot.Position();

            success = int.TryParse(tree.GetChild(0).Text, out position.Joint[0]);
            success = int.TryParse(tree.GetChild(1).Text, out position.Joint[1]);
            success = int.TryParse(tree.GetChild(2).Text, out position.Joint[2]);
            success = int.TryParse(tree.GetChild(3).Text, out position.Joint[3]);
            success = int.TryParse(tree.GetChild(4).Text, out position.Joint[4]);

            if (success)
            {
                robotController.EnqueueAction(r => MoveByCoordinateAction(position, r));
            }
        }

        /// <summary>
        /// Action for the MOVE_SORT token.
        /// </summary>
        /// <param name="tree">Tree containing parameters or sub-expression.</param>
        private void MoveToSortingLocation(CommonTree tree)
        {
            robotController.EnqueueAction(MoveToSortingLocationAction);
        }

        /// <summary>
        /// Action for the STOP token. 
        /// </summary>
        /// <param name="tree">Tree containing parameters or sub-expression.</param>
        private void StopMovement(CommonTree tree)
        {
            Contract.Requires(tree != null);

            char robotAxis;
            if (char.TryParse(tree.GetChild(0).Text, out robotAxis))
            {
                robotController.EnqueueAction(r => StopMovementAction(robotAxis, r));
            }
        }

        /// <summary>
        /// Action for the ROLL token.
        /// </summary>
        /// <param name="tree">Tree containing parameters or sub-expression.</param>
        private void RollConveyorBelt(CommonTree tree)
        {
            Contract.Requires(tree != null);

            int velocity;

            if (int.TryParse(tree.GetChild(0).Text, out velocity))
            {
                robotController.EnqueueAction(r => RollConveyorBeltAction(velocity, r));
            }
        }

        /// <summary>
        /// Action for the OPEN_GRIPPER token.
        /// </summary>
        /// <param name="tree">Tree containing parameters or sub-expression.</param>
        private void OpenGripper(CommonTree tree)
        {
            robotController.EnqueueAction(OpenGripperAction);
        }

        /// <summary>
        /// Action for the CLOSE_GRIPPER token.
        /// </summary>
        /// <param name="tree">Tree containing parameters or sub-expression.</param>
        private void CloseGripper(CommonTree tree)
        {
            robotController.EnqueueAction(CloseGripperAction);
        }

        /// <summary>
        /// Action for the MEASURE_JAW token.
        /// </summary>
        /// <param name="tree">Tree containing parameters or sub-expression.</param>
        private void MeasureJaw(CommonTree tree)
        {
            Contract.Requires(tree != null);

            robotController.EnqueueAction(r => MeasureJawAction(r, tree.GetChild(0).Text));
        }

        /// <summary>
        /// Action for the MEASURE_WEIGHT token.
        /// </summary>
        /// <param name="tree">Tree containing parameters or sub-expression.</param>
        private void MeasureWeight(CommonTree tree)
        {
            robotController.EnqueueAction(MeasureWeightAction);
        }

        #endregion

        #region ScorBotScript Expression Actions

        /// <summary>
        /// Action for <see cref="M:MoveByVector"/>.
        /// </summary>
        /// <param name="vectorName">Name of the vector.</param>
        /// <param name="position">The position.</param>
        /// <param name="robotController">The robot controller.</param>
        private void MoveByVectorAction(string vectorName, Data.Position position,
            IRobotController robotController)
        {
            Contract.Requires(!string.IsNullOrEmpty(vectorName));
            Contract.Requires(position != null);

            var robotPosition = ConvertToRobotPosition(position);
            if (robotPosition != null)
            {
                robotController.MoveToPosition(robotPosition,
                    PointType.AbsoluteJointCoordinate);
            }

            OnActionExecuted("Moving the robot to vector '{0}'.", vectorName);
        }

        /// <summary>
        /// Action for <see cref="M:MoveByCoordinate"/>.
        /// </summary>
        /// <param name="position">The position.</param>
        /// <param name="robotController">The robot controller.</param>
        private void MoveByCoordinateAction(Robot.Position position, IRobotController robotController)
        {
            robotController.MoveToPosition(position, PointType.RelativeJointCoordinate);

            OnActionExecuted("Moving the robot to position [{0},{1},{2},{3},{4}].",
                position.Joint[0],
                position.Joint[1],
                position.Joint[2],
                position.Joint[3],
                position.Joint[4]);
        }

        /// <summary>
        /// Action for <see cref="M:MoveToSortingLocation"/>.
        /// </summary>
        /// <param name="robotController">The robot controller.</param>
        private void MoveToSortingLocationAction(IRobotController robotController)
        {
            measuredItem.ActualDensity = CalculateDensity(measuredItem);
            measuredItem.TimeMeasured = DateTime.Now;

            var itemClassification = itemClassificationRepository.GetPositionByDensity(measuredItem.ActualDensity);

            if (itemClassification != null)
            {
                measuredItem.ItemClassification = itemClassification;

                measuredItemRepository.Add(measuredItem).SaveChanges();

                OnItemAdded(measuredItem);

                var robotPosition = ConvertToRobotPosition(itemClassification.Position);
                if (robotPosition != null)
                {
                    robotController.MoveToPosition(robotPosition, PointType.AbsoluteJointCoordinate);

                    OnActionExecuted("Moving to the {0} inventory container.",
                        itemClassification.Description);

                    measuredItem = new MeasuredItem();
                }
            }
        }

        /// <summary>
        /// Action for <see cref="M:StopMovement"/>.
        /// </summary>
        /// <param name="robotAxis">The robot axis.</param>
        /// <param name="robotController">The robot controller.</param>
        private void StopMovementAction(char robotAxis, IRobotController robotController)
        {
            robotController.StopMovement((Axis)robotAxis);

            OnActionExecuted("Stopping movement on axis '{0}'.",
                Enum.GetName(typeof(Axis), (Axis)robotAxis));
        }

        /// <summary>
        /// Action for <see cref="M:RollConveyorBelt"/>.
        /// </summary>
        /// <param name="velocity">The velocity.</param>
        /// <param name="robotController">The robot controller.</param>
        private void RollConveyorBeltAction(int velocity, IRobotController robotController)
        {
            robotController.WaitForConveyorBelt();
            robotController.MoveConveyorBelt(velocity);

            OnActionExecuted("Moving the conveyorbelt at a velocity of {0}.", velocity);
        }

        /// <summary>
        /// Action for <see cref="M:OpenGripper"/>.
        /// </summary>
        /// <param name="robotController">The robot controller.</param>
        private void OpenGripperAction(IRobotController robotController)
        {
            OnActionExecuted("Opening the gripper...");
            robotController.OpenGripper();
        }

        /// <summary>
        /// Action for <see cref="M:CloseGripper"/>.
        /// </summary>
        /// <param name="robotController">The robot controller.</param>
        private void CloseGripperAction(IRobotController robotController)
        {
            OnActionExecuted("Closing the gripper...");
            robotController.CloseGripper();
        }

        /// <summary>
        /// Action for <see cref="M:MeasureJaw"/>.
        /// </summary>
        /// <param name="robotController">The robot controller.</param>
        /// <param name="text">The text.</param>
        private void MeasureJawAction(IRobotController robotController, string text)
        {
            var jaw = robotController.Jaw;

            switch (text)
            {
                case "\"ActualWidth\"":
                    measuredItem.ActualWidth = jaw.Item2;
                    break;
                case "\"ActualHeight\"":
                    measuredItem.ActualHeight = jaw.Item2;
                    break;
                case "\"ActualDepth\"":
                    measuredItem.ActualDepth = jaw.Item2;
                    break;
            }

            OnActionExecuted("Measured the jaws of the '{1}' (Gap: {0} mm.)", jaw.Item2, text);
        }

        /// <summary>
        /// Action for <see cref="M:MeasureWeight"/>.
        /// </summary>
        /// <param name="robotController">The robot controller.</param>
        private void MeasureWeightAction(IRobotController robotController)
        {
            measuredItem.ActualWeight = scale.Weight;
            OnActionExecuted("Measured the weight (Wegith: {0} grams.)", measuredItem.ActualWeight);

            robotController.TriggerActionCompleted();
        }

        #endregion

        #region IScorBotScriptAsync Members

        private void ContinueCallback(AggregateException exception, Action<ScorBotScriptException> callback)
        {
            if (exception != null && exception.InnerExceptions.Count > 0)
            {
                callback(exception.InnerExceptions[0] as ScorBotScriptException);
            }
            else
            {
                callback(null);
            }
        }

        public void ExecuteScriptAsync(string script, Action<ScorBotScriptException> callback = null)
        {
            var task = Task.Factory.StartNew(() => ExecuteScript(script));

            if (callback != null)
            {
                task.ContinueWith(t => ContinueCallback(t.Exception, callback));
            }
        }

        public void ExecuteScriptAsync(TextReader reader, Action<ScorBotScriptException> callback = null)
        {
            var task = Task.Factory.StartNew(() => ExecuteScript(reader));

            if (callback != null)
            {
                task.ContinueWith(t => ContinueCallback(t.Exception, callback));
            }
        }

        public void CheckScriptAsync(string script, Action<ScorBotScriptException> callback = null)
        {
            var task = Task.Factory.StartNew(() => CheckScript(script));

            if (callback != null)
            {
                task.ContinueWith(t => ContinueCallback(t.Exception, callback));
            }
        }

        public void CheckScriptAsync(TextReader reader, Action<ScorBotScriptException> callback = null)
        {
            var task = Task.Factory.StartNew(() => CheckScript(reader));

            if (callback != null)
            {
                task.ContinueWith(t => ContinueCallback(t.Exception, callback));
            }
        }

        #endregion
    }
}
